import lejos.nxt.*;
import lejos.robotics.subsumption.*;

public class FrontWall implements Behavior {
	private UltrasonicSensor sonar;
	private boolean suppressed = false;

	public FrontWall(SensorPort port)
	{
		sonar = new UltrasonicSensor(port);
	}

	public boolean takeControl() {
		return (sonar.getDistance() < 25) && (Motor.B.getTachoCount() == 0);
	}

	public void suppress() {
		suppressed = true;
	}

	public void action() {
		suppressed = false;
		LCD.clear(3);
		LCD.drawString("Front wall detected", 0, 3);
		try {
			Thread.sleep(2000);
		} catch (InterruptedException e) {
			// TODO Auto-generated catch block
			e.printStackTrace();
		}
		Motor.B.rotate(-90, true);

		while( Motor.B.isMoving() && !suppressed )
			Thread.yield();

		Motor.B.stop();
	}
}
